#include "wheel_factor.h"

WheelFactor::WheelFactor(std::shared_ptr<WheelPreIntegration> wheel_preintegration) : wheel_preintegration_(wheel_preintegration) {

}

void WheelFactor::ComputeResidualAndJacobianPoseGraph(const State& last_state, const State& state, Eigen::Matrix<double, 3, 1> &residual, Eigen::Matrix<double, 3, 8> &jacobianXi, Eigen::Matrix<double, 3, 5> &jacobianXj) {
    Eigen::Matrix2d Ri = SO2(last_state.R).matrix();
    Eigen::Vector2d ri = last_state.p;
    double ai = SO2(last_state.R).log();
    double aj = SO2(state.R).log();
    Eigen::Vector2d rj = state.p;
 
    // 公式（24）
    // residual.head<2>() = Ri.transpose() * (rj-ri) - wheel_preintegration_->dp_;
    // residual[2] = SO2(aj - ai - wheel_preintegration_->dR_.log()).log(); // - ai - wheel_preintegration_->dR_.log()
    
    residual[0] = SO2(aj - ai - wheel_preintegration_->dR_.log()).log();
    residual.tail<2>() = Ri.transpose() * (rj-ri) - wheel_preintegration_->dp_;

    cout << " wheel residual ==== : " << residual.transpose() << std::endl;
    
    // compute jacobian matrix
    Eigen::Vector2d rij = rj-ri;
    Eigen::Vector2d rij_x(-rij[1], rij[0]);
    // 公式（25）, （十）--- 2.2
    // ti, vxi, vyi, pxi, pyi, bgi, baxi, bayi, tj, vxj, vyj, pxj, pyj; right
    jacobianXi.setZero();
    // jacobianXi.block<2,2>(0,0) = -Ri.transpose(); // ep/pi
    // jacobianXi.block<2,1>(0,4) = -Ri.transpose() * rij_x; // ev/ti 
    // jacobianXi(2,4) = -1;  // et/ti
 
    // jacobianXj.setZero();
    // jacobianXj.block<2,2>(0,0) = Ri.transpose(); 
    // jacobianXi(2,4) = 1;

    jacobianXi(0,0) = -1;  // et/ti
    jacobianXi.block<2,1>(1,0) = -Ri.transpose() * rij_x; // ep/ti 
    jacobianXi.block<2,2>(1,3) = -Ri.transpose(); // ep/p  

    jacobianXj.setZero();
    jacobianXj(0,0) = 1;
    jacobianXj.block<2,2>(1,3) = Ri.transpose(); 
}